import isaaclab.sim as sim_utils
from isaaclab.actuators import ImplicitActuatorCfg
from isaaclab.assets.articulation import ArticulationCfg

##
# Configuration
##

SF_TRON1_CFG = ArticulationCfg(
    spawn=sim_utils.UsdFileCfg(
        usd_path="/home/young/legged_lab/source/legged_lab/legged_lab/assets/data/SF_TRON1/SF_TRON1.usd",
        activate_contact_sensors=True,
        rigid_props=sim_utils.RigidBodyPropertiesCfg(
            disable_gravity=False,
            retain_accelerations=False,
            linear_damping=0.0,
            angular_damping=0.0,
            max_linear_velocity=1000.0,
            max_angular_velocity=1000.0,
            max_depenetration_velocity=1.0,
        ),
        articulation_props=sim_utils.ArticulationRootPropertiesCfg(
            enabled_self_collisions=True, solver_position_iteration_count=4, solver_velocity_iteration_count=0
        ),
    ),
    init_state=ArticulationCfg.InitialStateCfg(
        pos=(0.0, 0.0, 0.82),
        joint_pos={
            "abad_L_Joint": 0.0,
            "hip_L_Joint": 0.0,
            "knee_L_Joint": 0.0,
            "ankle_L_Joint": 0.0,
            "abad_R_Joint": 0.0,
            "hip_R_Joint": 0.0,
            "knee_R_Joint": 0.0,
            "ankle_R_Joint": 0.0,
        },
        joint_vel={".*": 0.0},
    ),
    #soft_joint_pos_limit_factor=0.9,
    actuators={
        "legs": ImplicitActuatorCfg(
            joint_names_expr=["abad_.*", "hip_.*", "knee_.*"],
            effort_limit_sim=80.0,
            velocity_limit_sim=15.0,
            stiffness={
                "abad_.*": 40.0,
                "hip_.*": 40.0,
                "knee_.*": 40.0,
            },
            damping={
                "abad_.*": 1.5,
                "hip_.*": 1.5,
                "knee_.*": 1.5,
            },
        ),
        "ankles": ImplicitActuatorCfg(
            joint_names_expr=["ankle_.*"],
            effort_limit_sim=20.0,
            velocity_limit_sim=15.0,
            stiffness={
                "ankle_.*": 0.0,
            },
            damping={
                "ankle_.*": 0.0,
            },
        ),

    },
)
